/* 
 * File:   usbsample.c
 * Author: Victor Nidens
 *
 * Created on March 9, 2012, 5:37 PM
 */

#include <stdio.h>
#include <stdlib.h>
#include "string.h"
#include "nxtusb/usbcomm.h"
#include "libusb-1.0/libusb.h"
#include "nxtusb/nxt.h"

/*
 * 
 */
int main(int argc, char** argv) {

	libusb_device_handle *deviceHandle[5];
	uint8 devicesToConnect = 5;
	GenericResponse resp;
	BatteryState battery;
	MotorOutputState motor;
	DeviceSystemInfo info;
	SensorState sensor;
	uint8 sensorId = 0x01;
	int i = 0;

	memset(&resp, 0, sizeof(GenericResponse));

	initUSB();

	connectNXTDevices(deviceHandle, &devicesToConnect);

	if(deviceHandle[0] == NULL){
		return 1;
	}

	nxtPlaySound(&deviceHandle[0], 333, 100, 1, &resp);

	nxtGetBatteryLevel(&deviceHandle[0], &battery);

	printf("Battery.rval = %i\nBattery.status = %i\nBattery.state = %i\n\n", battery.rval, battery.status, battery.batteryState);

	nxtMotorsRotate(&deviceHandle[0],
						NXT_MOTOR_A,
						50,
						NXT_MOTOR_ON|NXT_REGULATED,
						NXT_REGULATION_MODE_MOTOR_SPEED,
						0,
						NXT_MOTOR_RUN_STATE_RUNNING,
						0,
						1,
						&resp);

	nxtPlaySound(&deviceHandle[0], 333, 100, 1, &resp);

	nxtMotorsRotate(&deviceHandle[0], NXT_MOTOR_A, 100, NXT_MOTOR_ON|NXT_REGULATED, NXT_REGULATION_MODE_MOTOR_SPEED, 0, NXT_MOTOR_RUN_STATE_RUNNING, 200, 1, &resp);

	nxtGetMotorState(&deviceHandle[0], NXT_MOTOR_A, &motor);

	printf("::MotorState struct:::\n");
	printf("motorState.rval = %i\n", motor.rval);
	printf("motorState.status = %i\n", motor.status);
	printf("motorState.rotations = %i\n", motor.rotation_count);
	printf("motorState.tacho_count = %i\n", motor.tacho_count);
	printf("motorState.power = %i\n", motor.power);
	printf("motorState.tachoLimit = %i\n", motor.tacho_limit);




	for(; sensorId <= 8; sensorId++){
		nxtSetInputMode(&deviceHandle[0], NXT_SENSOR_1, sensorId, NXT_RAW_MODE, NXT_RET, &resp);
		for(i = 0; i < 15; i++){
			memset(&sensor, 0, sizeof(SensorState));
			nxtGetInputValues(&deviceHandle[0], NXT_SENSOR_1, &sensor);
			printf("Received some value of sensor with Id = [%i]\n", sensorId);
			printf("Raw value = [%i]\n", sensor.raw_value);
			printf("Normal value = [%i]\n", sensor.normal_value);
			printf("Scaled value = [%i]\n\n", sensor.scaled_value);
		}
	}

	nxtGetDeviceSystemInfo(&deviceHandle[0], &info);

	freeUSB(&deviceHandle[0]);

	printf("Normal exit");
	return 0;
}


